/*!
* @file Coordinate.cpp
* @brief 三维坐标系
* @date 2024/9/12
*/
#include "Coordinate.h"
#include "Placement.h"
#include "Matrix4D.h"

using namespace data;

Coordinate::Coordinate(const Axis& axis, const Vector3d& xd)
{
    SetAxes(axis, xd);
}

void Coordinate::SetAxis(const Axis& axis)
{
    SetAxes(axis, x);
}

void Coordinate::SetAxes(const Axis& axis, const Vector3d& xd)
{
    const Vector3d& n = axis.dir;
    Vector3d yd = n % xd;
    if (yd.Sqr() < traits_type::epsilon())
        return;

    y = yd;
    y.Normalize();
    x = y % n;
    x.Normalize();
    this->axis.Set(axis.base, n.Normalized());
}

void Coordinate::SetAxes(const Vector3d& n, const Vector3d& xd)
{
    Vector3d yd = n % xd;
    if (yd.Sqr() < traits_type::epsilon())
        return;

    y = yd;
    y.Normalize();
    x = y % n;
    x.Normalize();
    axis.dir = n.Normalized();
}

const Vector3d& Coordinate::Position() const
{
    return axis.base;
}

void Coordinate::SetPosition(const Vector3d& pos)
{
    axis.base = pos;
}

const Vector3d& Coordinate::X() const
{
    return x;
}

void Coordinate::SetX(const Vector3d& dir)
{
    Vector3d yd = axis.dir % dir;
    if (yd.Sqr() < traits_type::epsilon())
        return;

    y = yd;
    y.Normalize();
    x = y % axis.dir;
    x.Normalize();
}

const Vector3d& Coordinate::Y() const
{
    return y;
}

void Coordinate::SetY(const Vector3d& dir)
{
    Vector3d xd = dir % axis.dir;
    if (xd.Sqr() < traits_type::epsilon())
        return;

    x = xd;
    x.Normalize();
    y = axis.dir % x;
    y.Normalize();
}

const Vector3d& Coordinate::Z() const
{
    return axis.dir;
}

void Coordinate::SetZ(const Vector3d& dir)
{
    SetAxes(dir, x);
}

void Coordinate::SetPlacement(const Placement& plm)
{
    auto& rotation = plm.rotation;
    axis.base = plm.position;
    axis.dir = Vector3d(0, 0, 1) * rotation;
    x = Vector3d(1, 0, 0) * rotation;
    y = Vector3d(0, 1, 0) * rotation;
}

void Coordinate::Transform(const Placement& plm)
{
    axis *= plm;
    x *= plm;
    y *= plm;
}

void Coordinate::Transform(const Rotation& rot)
{
    axis.dir *= rot;
    x *= rot;
    y *= rot;
}

Vector3d Coordinate::ToLocalPoint(const Vector3d& pnt)
{
    Vector3d offset = pnt - axis.base;
    return Vector3d(x * offset, y * offset, axis.dir * offset);
}

Vector3d Coordinate::ToWorldPoint(const Vector3d& pnt)
{
    return axis.base + x * pnt.x + y * pnt.y + axis.dir * pnt.z;
}
